/******************************************************************************\
 *                                                                            *
 *                           Define Math Stuff                                *
 *                   Flavio Bertini fl.bertini@gmail.com                      *
 *                                                                            *
 *                                                                            *
\******************************************************************************/

#if !defined(__Math_h__)
#define __Math_h__

#include <vector>
#include <cassert>
#include "LeapMath.h"

using namespace std;
using namespace Leap;

#define SQR(t)    ((t)*(t))
#define SEGNO(a)  (a>0 ? 1 : (a==0 ? 0 : -1))
/* Calcola la distanza in 3D tra due punti */
#define DIST_3D(a,b)  (sqrt(SQR((a).x-(b).x) + SQR((a).y-(b).y) + SQR((a).z-(b).z)))
#define SWAP(a,b)\
        {\
        double _temp;\
        _temp = *(a);\
        *(a) = *(b);\
        *(b) = _temp;\
        }
/* Copia di coordinate in 3D di un punto */
#define COPY_3D(a,b)\
        (a).x = (b).x;\
        (a).y = (b).y;\
        (a).z = (b).z;


// New Vector struct
struct Mat_Vector : Vector
{
  public:
  // spherical coordinate
  float rho, phi, theta;

  // creates a new empty Mat_Vector
  Mat_Vector();

  // creates a new Mat_Vector starting from a Vector
  Mat_Vector(const Vector vector);

  // creates a new Vector with the specified component values
  Mat_Vector(float _x, float _y, float _z);

  // set zero all coordinates
  void mat_setZero(void);

  // transform cartesian coordinates to spherical coordinates
  void mat_cartesian2sphere(void);

  // transform spherical coordinates to cartesian coordinates
  void mat_sphere2cartesian();

};  // New Vector struct


// Useful functions
// check if two floats are almost equal
bool mat_almostEqual(float a, float b, int maxUlps);

// translate the point A to the point B by transl
void mat_translateVector(Mat_Vector pointA, Mat_Vector transl, Mat_Vector &pointB);
void mat_translateVector(float pointA[3], Mat_Vector transl, float pointB[3]);

// multiply vector-matrix
void mat_vectorMatrixMultiply(float pointA[3], float M[16], float pointB[3]);

// compute normal between three points
Vector mat_computeNormal(vector<Vector> triangle);

// compute normal between three points array
Vector mat_computeNormal(float triangle[9]);

// load identity
void mat_glMatrixfIdentity(float M[16]);

// invert matrix
int mat_glMatrixInvert(float src[16], float dst[16]);

// multiply matrix
void mat_glMatrixfMultiply(float A[16], float B[16], float C[16]);

// compute the rotation matrix (trackball theory)
void mat_computeRotationMatrix(Mat_Vector v1, Mat_Vector v2, float M[16]);

// compute the rotation matrix
void mat_computeRotationMatrixFix(Vector v, float alpha, float M[16]);

// rotation defined around coordinate axes
void mat_glMatrixXrot(float angle, float M[16]);
void mat_glMatrixYrot(float angle, float M[16]);
void mat_glMatrixZrot(float angle, float M[16]);

// rotate the vector around y axis
void mat_glRotateXZ(float a, Vector *v);

/* Effettua l'intersezione del triangolo di vertici v1, v2, v3 con il
   segmento di estremi est1, est2. Restituisce in trian ed in seg
   rispettivamente le coordinate del punto di intersezione trovato nel
   triangolo e nel segmento. fd sara' uguale a true se e' stata trovata una
   intersezione, a false altrimenti. */
void mat_intersection(Vector *v1, Vector *v2, Vector *v3, Vector *est1, Vector *est2,Vector *trian, Vector *seg, bool *fd);

/* Perpendicolare tre due vettori */
void mat_perp(Vector *p1, Vector *p2, double a, double b, double c, double *x, double *y, double *z);

/* Segno del prodotto scalare tra due vettori:
   <0 : >90° ;
   =0 : =90° ;
   >0 : <90° . */
void mat_piano(Vector *vert1, Vector *vert2, double a, double b, double c, int *k);

double mat_lambda(Vector *vert1, Vector *vert2, Vector *interno);

/* Distanza tra due punti */
double mat_pointDistance(Vector v1,Vector v2);


#endif // __Math_h__
